#include "pid.h"
#include <math.h>

#define M_PI	(3.141592653589793)
#define M_2PI	(M_PI * 2)

static float float_mid(float i, float max, float min);

float calc_lowpass_alpha_dt(float dt, float cutoff_freq)
{
	float rc;
	if (dt <= 0.0f || cutoff_freq <= 0.0f) {
			return 1.0;
	}
	rc = 1.0f/(M_2PI*cutoff_freq);
	return float_mid(dt/(dt+rc), 0.0f, 1.0f);
}

static float float_mid(float i, float max, float min)
{
	if(i<min)
		return min;
	else if(i>max)
		return max;
	return i;
}

unsigned char pid_update(PID* pid, float target, float current)
{
	float err=0.0f, D=0.0f;

	err = target-current;
	
	if(pid->reset)
	{
		pid->reset = 0;
		pid->last_error = err;		
		pid->isum = 0.0f;
		pid->output = 0.0f;
	}
	else
	{
		D = ((err-pid->last_error)/pid->dt)*pid->kd;
	}
	
	pid->output = err * pid->kp;
	
	pid->isum += err * pid->dt * pid->ki;
	pid->isum = float_mid(pid->isum, pid->imax, -pid->imax);
	
  pid->last_error = err;
	pid->output += pid->isum + D;
	
	return 1;
	
}

void pid_reset(PID* pid)
{
	pid->reset = 1;
	pid->output = 0.0f;
}
